// 使用Eigen对位姿进行插值，主要考察slerp()函数
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

struct Pose
{
    Eigen::Vector3d pos = Eigen::Vector3d::Zero();
    Eigen::Matrix3d rot = Eigen::Matrix3d::Identity();
    double time_stamp = 0.0;
};


// 调用Eigen的slerp函数，对四元数进行线性插值
void interpolation(const Pose& in_1, const Pose& in_2, double time_stamp_in, Pose out){
    double dt_1 = time_stamp_in - in_1.time_stamp;
    double dt_2 = in_2.time_stamp - time_stamp_in;
    double dt_3 = dt_1 + dt_2;
    out.pos = in_1.pos * (dt_2 / dt_3) + in_2.pos * (dt_1 / dt_3);
    Eigen::Quaterniond in_1_q(in_1.rot);
    Eigen::Quaterniond in_2_q(in_2.rot);
    Eigen::Quaterniond out_q = in_1_q.slerp(dt_1 / dt_3, in_2_q);
    out.rot = out_q.toRotationMatrix();
    out.time_stamp = time_stamp_in;
}




int main(int argc, char* argv[]){

    std::cout << "ok" << std::endl;
    return 0;
}




